#include <iostream>
#include <opencv2/opencv.hpp>

#include "commons/h264decoder.h"
#include "commons/h264encoder.h"
#include "commons/libar8020.h"

int main() {
  if (Usb_Init_So()<0) {
    printf({"error init use\n"});
    return -1;
  }

  cv::Mat frame;
  cv::Mat dst;

  cv::VideoCapture cap("/home/robot/zhangyiyang/catkin_ws/src/model_prediction/tests/data/whj.mp4");
  assert(cap.isOpened());

  h264Encoder h264;
  AvH264EncConfig conf;
  conf.width = 640;
  conf.height = 480;
  conf.gop_size = 10;
  conf.max_b_frames = 0;
  conf.frame_rate = (int)cap.get(cv::CAP_PROP_FPS);
  h264.Init(conf);

  CH264Decoder m_h264Decoder;
  m_h264Decoder.initial();

  int jpeg_quality = 100;
  std::vector<int> params;
  params.push_back(cv::IMWRITE_JPEG_QUALITY);
  params.push_back(jpeg_quality);

  cv::Mat cvDst;
  int nWaitTime = 1;
  while (1) {
    cap >> frame;
    if (frame.empty()) break;

    cv::imshow("src", frame);
    cv::Mat _frame;
    cv::resize(frame, _frame, cv::Size(640, 480));
    cv::imshow("reisze", _frame);

    double timePoint1 = cv::getTickCount();

    // std::vector<uchar> jpgSize;
    // cv::imencode(".jpg", _frame, jpgSize, params);
    // double timePoint2 = cv::getTickCount();

    // do encode
    AVPacket* pkt = h264.encode(_frame);
    int size = pkt->size;
    uchar* data = nullptr;
    data = pkt->data;
    double timePoint3 = cv::getTickCount();

    // send
    Video_Port_Send_So((char*)data, size);
    double timePoint4 = cv::getTickCount();

    // // do decode
    // m_h264Decoder.decode(data, size, dst);
    // cv::imshow("target", dst);

    // printf("cv::encode size:%d Fps: %.2f\n", jpgSize.size(), cv::getTickFrequency() / (timePoint2 - timePoint1));
    printf("h264 encode size:%d, Fps: %.2f\n", size, cv::getTickFrequency() / (timePoint3 - timePoint1));
    printf("send Fps: %.2f\n", size, cv::getTickFrequency() / (timePoint4 - timePoint3));

    char chKey = cv::waitKey(nWaitTime);
    // ESC
    if (27 == chKey)
      break;
    else if (' ' == chKey)
      nWaitTime = !nWaitTime;
  }

  return 0;
}